//==============================================================================
//
//  File        : sensor_CG2355.c
//  Description : Firmware Sensor Control File
//  Author      : Philip Lin
//  Revision    : 1.0
//
//=============================================================================

#include "includes_fw.h"
#if defined(DSC_MP3_P_FW)||defined(VIDEO_AMR_R_FW)||defined(VIDEO_AAC_R_FW)||(SENSOR_EN)
#include "config_fw.h"

#if BIND_SENSOR_CG2355

#include "lib_retina.h"
#include "reg_retina.h"

#include "mmp_reg_vif.h"
#include "mmp_reg_scaler.h"
#include "mmp_reg_gpio.h"
#include "mmp_reg_rawproc.h"

#include "mmpf_sensor.h"
#include "mmpf_hif.h"
#include "mmpf_i2cm.h"
#include "mmpf_vif.h"
#include "isp_if.h"
#include "mmpf_usbpccam.h"

#include "mmp_reg_i2cm.h"
#include "mmps_3gprecd.h"
#include "sensor_CG2355.h"
#include "hdm_ctl.h"
#include "mmpf_dram.h"
#include "motor_ctl.h"
#include "flash_ctl.h"
#include "mmpf_pll.h"

static MMPF_I2CM_ATTRIBUTE m_snr_I2cmAttribute = {
            SENSOR_I2CM_ID, SENSOR_I2C_ADDR_CG2355, 8, 8, 0, MMP_FALSE, MMP_FALSE, MMP_FALSE,
            MMP_FALSE, 0, 0, 0, MMPF_I2CM_SPEED_HW_250K, NULL, NULL};

#if (CUSTOMER == ASU)
#if (SENSOR_IF != SENSOR_IF_PARALLEL)
    #undef SENSOR_IF 
    #define SENSOR_IF                (SENSOR_IF_PARALLEL)
#endif
#endif
            
#ifndef SENSOR_ID_CG2355
    #define SENSOR_ID_CG2355 0
#endif

MMP_ULONG g_SNR_LineCntPerSec = 1;

static MMP_UBYTE   vif_id = 0;

extern MMP_USHORT  gsSensorPreviewWidth[], gsSensorPreviewHeight[];
extern MMP_USHORT  gsCurPreviewMode, gsCurPhaseCount;
//extern MMP_USHORT  gsSensorMode;
extern MMP_USHORT  m_usVideoQuality;
extern MMP_USHORT  m_gsISPCoreID;

extern MMPS_3GPRECD_VIDEO_FORMAT m_VideoFmt;
extern MMP_BOOL    gbAutoFocus;

extern MMP_USHORT m_usResolType;
extern MMP_ULONG   glPccamResolution;

MMP_LONG gldummy[5] = {0};

MMP_USHORT snr_1, snr_2, snr_3, snr_4, snr_5;

#if (IQ_TABLE_LOAD_FROM_FLASH == 0)
ISP_UINT8 Sensor_IQ_CompressedText[] = {
#include "isp_8451_iq_data_v2_CG2355.xls.iq.txt"
};
#endif

#define  SENSOR_ROTATE_180    0   

#define BASE_SHUTTER_50HZ		248
#define BASE_SHUTTER_60HZ		207

void SNR_CG2355_SetSensorResolution_Customer(ISP_SENSOR_RESOL res);
void SNR_CG2355_InitSensor_Customer(void);
#if SUPPORT_AUTO_FOCUS
void MOTOR_CG2355_VCM_InitMotor_Customer(void);
void MOTOR_CG2355_VCM_SetAFPosition_Customer(MMP_USHORT af_pos);
#endif

void MMPF_SetPrivacyMask(ISP_UINT8 en, ISP_UINT8* privacymask);
ISP_UINT8 en_flag, privacymask[35];
static VENUS_SYSTEM_MODE gSystemMode;

//------------------------------------------------------------------------------
//  Function    : CG2355_Sensor_PreviewMode
//  Description :
//------------------------------------------------------------------------------
extern MMP_BYTE gbSignalType;
extern MMPF_SENSOR_FUNCTION *gsSensorFunction ;
static void CG2355_Sensor_PreviewMode(MMP_USHORT usPreviewmode)
{
    gsCurPreviewMode = usPreviewmode;
    ISP_IF_AE_SetMinFPSx10(150);
    switch (usPreviewmode) {
    case 0:
        RTNA_DBG_Str(3, "Sensor 5M (1920x1080) preview mode\r\n");
        //ISP_IF_3A_Control(ISP_3A_DISABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_FALSE);

        gSystemMode = VENUS_SYSTEM_MODE_SNAPSHOT;//ISP_IF_SNR_SetBeforeCapture();
        ISP_IF_IQ_SetSysMode(0); //Steven NOTE:please reference excel define: 0=capture, 1=preview, 2=video....
        SNR_CG2355_SetSensorResolution_Customer(ISP_SENSOR_RESOL_1920x1080);

        //set color transform 16 ~ 235
        MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_1, MMP_TRUE, MMPF_SCALER_COLRMTX_BT601);

        ISP_IF_AE_SetFPS(30);
        //ISP_IF_3A_Control(ISP_3A_ENABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_TRUE);
        break;

    case 1:
        RTNA_DBG_Str(0, "Sensor preview (1600x1200) mode\r\n");
        RTNA_DBG_Str(0, "SENSOR_VIDEO_MODE\r\n");
        //ISP_IF_3A_Control(ISP_3A_DISABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_FALSE);

        gSystemMode = VENUS_SYSTEM_MODE_PREVIEW;//ISP_IF_SNR_SetBeforePreview();
        ISP_IF_IQ_SetSysMode(1); //Steven NOTE:please reference excel define: 0=capture, 1=preview, 2=video....
        SNR_CG2355_SetSensorResolution_Customer(ISP_SENSOR_RESOL_1600x1200);

#if 0	
        MMPF_VIF_GetVIFGrabResolution(&ulWidth, &ulHeight);
        dbg_printf(3, "[2] ulWidth = %d, ulHeight = %d\r\n", ulWidth, ulHeight); 
#endif

        //set color transform 16 ~ 235
        MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_1, MMP_TRUE, MMPF_SCALER_COLRMTX_BT601);

        ISP_IF_AE_SetFPS(30);

        //ISP_IF_3A_Control(ISP_3A_ENABLE);
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_TRUE);
        break;
    }
}
extern void GPIO_OutputControl(MMP_UBYTE num, MMP_UBYTE status);
extern volatile MMP_UBYTE   gbDramID;
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Initialize
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_Initialize_CG2355(void)
{
    AITPS_VIF   pVIF = AITC_BASE_VIF;
    RES_TYPE_CFG *cur_res;

    RTNA_DBG_Str(0, "MMPF_Sensor_Initialize_CG2355...Begin\r\n");

    //Init globol variable used by DSC
    gsSensorPreviewWidth[0] = 1600-8;
    gsSensorPreviewHeight[0] = 1200-8;
    // use 1.3M for preview, check sensor preview setting
    gsSensorPreviewWidth[1] = 1600-8;
    gsSensorPreviewHeight[1] = 1200-8;

#if (CUSTOMER == IPC) || (CUSTOMER == QDR) || (CUSTOMER == ANW)
    MMPF_PIO_EnableOutputMode(SEN_RST, MMP_TRUE);
    MMPF_PIO_EnableOutputMode(SEN_PWR_EN, MMP_TRUE);
    MMPF_PIO_EnableGpioMode(SEN_RST, MMP_TRUE);
    MMPF_PIO_EnableGpioMode(SEN_PWR_EN, MMP_TRUE);
#endif

    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_TRUE);
        MMPF_OS_Sleep_MS(5);
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_FALSE);
    }
    else{
        MMPF_VIF_SetPIODir(VIF_SIF_SEN, MMP_TRUE);
        MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_TRUE);  //Pin output high
        MMPF_OS_Sleep_MS(5);
        //MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_FALSE);//Pin output low
    }

    GPIO_OutputControl(SEN_PWR_EN, TRUE);
    MMPF_OS_Sleep_MS(5);
    pVIF->VIF_SENSR_CLK_CTL[vif_id] = VIF_SENSR_CLK_EN;
    MMPF_OS_Sleep_MS(5);
    MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_FALSE);//Pin output low
    MMPF_OS_Sleep_MS(5);

    /*Reset Sensor 500mS*/
#if (CUSTOMER == IPC) || (CUSTOMER == QDR) || (CUSTOMER == ANW)
    MMPF_PIO_SetData(SEN_RST, MMP_FALSE);
    MMPF_OS_Sleep_MS(5);
    MMPF_PIO_SetData(SEN_RST, MMP_TRUE);
    MMPF_OS_Sleep_MS(5);
//#else
    MMPF_VIF_SetPIODir(VIF_SIF_RST, MMP_TRUE);
    MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_FALSE);
    MMPF_OS_Sleep_MS(5);
    MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_TRUE);
    MMPF_OS_Sleep_MS(5);
#endif
    SNR_CG2355_InitSensor_Customer();

    cur_res = GetResCfg(glPccamResolution);
    if(cur_res->res_w > gsSensorPreviewWidth[1] || cur_res->res_h > gsSensorPreviewHeight[1]) {
        gsSensorFunction->MMPF_Sensor_SetPreviewMode(0);
    }
    else{
        gsSensorFunction->MMPF_Sensor_SetPreviewMode(1);
    }

#if SUPPORT_AUTO_FOCUS
    MOTOR_CG2355_VCM_InitMotor_Customer();
#endif

#if SUPPORT_FLASH_LIGHT
    VR_Flash_Init();
#endif

    RTNA_DBG_Str(0, "MMPF_Sensor_Initialize_CG2355...End\r\n");
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_PowerDown_CG2355
//  Description :
//------------------------------------------------------------------------------

static MMP_ERR  MMPF_Sensor_PowerDown_CG2355(void)
{
    AITPS_VIF  pVIF = AITC_BASE_VIF;

    MMPF_VIF_EnableInputInterface(MMP_FALSE);

    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_TRUE);
    }
    else{
        MMPF_VIF_SetPIODir(VIF_SIF_SEN, MMP_TRUE);
        MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_TRUE);  //Pin output high
    }
    MMPF_OS_Sleep_MS(5);

    pVIF->VIF_SENSR_CLK_CTL[vif_id] &= (~ VIF_SENSR_CLK_EN) ;   //output sensor main clock


    #if (CUSTOMER == SNO)  
    GPIO_OutputControl(CAM_EN, FALSE);
    MMPF_OS_Sleep_MS(10);
    #endif
    #if (CUSTOMER == ANW)
    GPIO_OutputControl(SEN_PWR_EN, FALSE);
    #endif

    MMPF_OS_Sleep_MS(20);

    RTNA_DBG_Str3( "MMPF_Sensor_PowerDown_CG2355() \r\n");
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetFrameRate_CG2355
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_SetFrameRate_CG2355(MMP_UBYTE ubFrameRate)
{
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetPreviewMode
//  Description :
//------------------------------------------------------------------------------

static MMP_ERR MMPF_Sensor_SetPreviewMode_CG2355(MMP_USHORT usPreviewmode)
{
    CG2355_Sensor_PreviewMode(usPreviewmode);

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetPreviewMode
//  Description :
//------------------------------------------------------------------------------

static MMP_ERR MMPF_Sensor_ChangePreviewMode_CG2355(
						MMP_USHORT usCurPreviewmode, MMP_USHORT usCurPhasecount,
						MMP_USHORT usNewPreviewmode, MMP_USHORT usNewPhasecount)
{
    CG2355_Sensor_PreviewMode(usNewPreviewmode);

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_ChangeMode
//  Description :
//------------------------------------------------------------------------------

/*static*/ MMP_ERR MMPF_Sensor_ChangeMode_CG2355(MMP_USHORT usPreviewmode, MMP_USHORT usCapturemode)
{
    switch (usPreviewmode) {
    case 0:
        switch (usCapturemode) {
        case 0:
            break;
        case 1:
            break;
        }
        break;
    case 1:
        switch (usCapturemode) {
        case 0:// SXGA preview, 5M capture
            MMPF_Sensor_SetPreviewMode_CG2355(0);
            // wait 3 frame ...
            // MMPF_Sensor_WaitFrame(5);
            break;
        case 1:
            MMPF_Sensor_SetPreviewMode_CG2355(1);
            // wait 3 frame ...
            //MMPF_Sensor_WaitFrame(3);

            break;
        }
        break;
    }

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_InitializeVIF
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_InitializeVIF_CG2355(void)
{
    AITPS_MIPI  pMIPI = AITC_BASE_MIPI;
    AITPS_VIF  pVIF = AITC_BASE_VIF;
    ISP_HDM_VIF_IF vif_if;
    MMP_ULONG   ulSENSORCLK;

    #if (SENSOR_IF == SENSOR_IF_PARALLEL)
    MMPF_VIF_RegisterInputInterface(MMPF_VIF_PARALLEL);
    pVIF->VIF_SENSR_CTL[vif_id] = VIF_SENSR_LATCH_PST | VIF_VSYNC_POLAR_NEG | VIF_14BPP_OUT_EN;
    #else
    pMIPI->MIPI_DATA0_DLY[vif_id] = 0x1000;  // set SOT counter to 0x10
    pMIPI->MIPI_DATA1_DLY[vif_id] = 0x1000;  // set SOT counter to 0x10

    MMPF_VIF_RegisterInputInterface(MMPF_VIF_MIPI);
    #endif

	// initialize VIF OPRs
#if (SENSOR_IF == SENSOR_IF_PARALLEL)
    vif_if = ISP_HDM_VIF_IF_PARALLEL;		//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_PARALLEL, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_1_LANE)	
    vif_if = ISP_HDM_VIF_IF_MIPI_SINGLE_0;	//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_SINGLE_0, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_2_LANE)		
    vif_if = ISP_HDM_VIF_IF_MIPI_DUAL_01;	//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_DUAL_01, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_4_LANE)		
    vif_if = ISP_HDM_VIF_IF_MIPI_QUAD;		//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_QUAD, 22);
#endif	

    {
        pVIF->VIF_INT_HOST_EN[vif_id] = 0;
        pVIF->VIF_INT_CPU_SR[vif_id] = VIF_INT_ALL;
        pVIF->VIF_OUT_EN[vif_id] = 0;
        pVIF->VIF_RAW_OUT_EN[vif_id] = 0;
        pVIF->VIF_FRME_SKIP_NO[vif_id] = 0;
        pVIF->VIF_FRME_SKIP_EN[vif_id] = 0;

        //RTNA_DBG_Str(0,"InitializeVIF\r\n");
        MMPF_PLL_GetGroupFreq(4, &ulSENSORCLK);

        switch(ulSENSORCLK){
            case 96000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D4; // 24MHz
                RTNA_DBG_Str3("96MHz DIV 4\r\n");
                break;
            case 144000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D6; 
                RTNA_DBG_Str3("144MHz DIV 6\r\n");
                break;
            case 192000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D8;
                RTNA_DBG_Str3("192MHz DIV 8\r\n");
                break;
            case 216000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D9;
                RTNA_DBG_Str3("216MHz DIV 9\r\n");
                break;
            case 240000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D10;
                RTNA_DBG_Str3("240MHz DIV 10\r\n");
                break;
            case 120000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D5;
                RTNA_DBG_Str3("120MHz DIV 5\r\n");
                break;
            default:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D12;  // 144/12 = 12M
                break;
        }

        pVIF->VIF_SENSR_CTL[vif_id] = VIF_SENSR_LATCH_PST | VIF_LINE_ID_POLAR | VIF_PIXL_ID_POLAR;
        pVIF->VIF_YUV_CTL[vif_id] = VIF_YUV_FORMAT_VUY;
        //pVIF->VIF_SENSR_CLK_CTL[vif_id] = VIF_SENSR_CLK_EN;   //Move to Sensor initialize
        pVIF->VIF_OPR_UPD[vif_id] = VIF_OPR_UPD_EN | VIF_OPR_UPD_FRAME;

        if (vif_if==ISP_HDM_VIF_IF_PARALLEL) {
            RTNA_DBG_Str0("ISP_HDM_VIF_IF_PARALLEL\r\n");
        }
        else {
            pMIPI->MIPI_DATA0_CFG[vif_id] = ((pMIPI->MIPI_DATA0_CFG[vif_id] & ~MIPI_DAT_SRC_SEL_MASK) | MIPI_DAT_SRC_PHY_1) | MIPI_CSI2_EN;
            if (vif_if == ISP_HDM_VIF_IF_MIPI_DUAL_01) {
                pMIPI->MIPI_DATA1_CFG[vif_id] = ((pMIPI->MIPI_DATA1_CFG[vif_id] & ~MIPI_DAT_SRC_SEL_MASK) | MIPI_DAT_SRC_PHY_2) | MIPI_CSI2_EN;
            }
            #if (CHIP == P_V2)
            else if(vif_if==ISP_HDM_VIF_IF_MIPI_QUAD){
                pMIPI->MIPI_DATA2_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_2;
                pMIPI->MIPI_DATA3_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_3;
                pMIPI->MIPI_DATA4_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_4;
            }
            #endif
        }
    }


    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_InitializeISP
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_InitializeISP_CG2355(void)
{
    // register sensor (functions and variables)
    RTNA_DBG_Str(0, "ISP_IF_LIB_Init...\r\n");
    // check if ISP Lib and IQ are mismatched.
    ISP_IF_LIB_Init();
    // check if ISP Lib and IQ are mismatched.
    if (ISP_IF_LIB_CompareIQVer() != MMP_ERR_NONE) {
        RTNA_DBG_Str(0, "Wrong ISP lib version!\r\n");
        return MMP_ERR_NONE;
    }

    //RTNA_DBG_Str(0, "ISP_IF_3A_Init...S\r\n");
    // initialize 3A
    ISP_IF_3A_Init();

    // set AF type (ISP_AF_TYPE_ONE_SHOT / ISP_AF_TYPE_CONTINUOUS)
    ISP_IF_AF_SetType(ISP_AF_TYPE_CONTINUOUS);
    //RTNA_DBG_Str(0, "ISP_IF_3A_Init...E\r\n");

    //ISP_IF_AE_SetLowLuxMode(0);//disable Low light Compensation

    return  MMP_ERR_NONE;
}

//============================================================================

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetAFPosition_CG2355
//  Description :
// ---------------------------------------
static MMP_ERR MMPF_Sensor_SetAFPosition_CG2355(MMP_UBYTE ubPos)
{
#if SUPPORT_AUTO_FOCUS
    //if (gISPConfig.AutoFocusMode == VENUS_AF_MODE_MANUAL) {
        //gAFConfig.AFPos = ubPos;
        ISP_IF_AF_SetPos(ubPos, 8);
        MOTOR_CG2355_VCM_SetAFPosition_Customer(ISP_IF_AF_GetPos(0));
    //}

    //RTNA_DBG_Str(3, "  ubPos = ");       
    //RTNA_DBG_Short(3, ubPos);
    //RTNA_DBG_Str(3, "\r\n");
#endif
    return  MMP_ERR_NONE;
}
//ISP_UINT32 gISPFrameCnt;
#define MinFrameDivsor 5 // Gason@20120111, by OV suggestion, under 15 fps, dummy line will work every 5 frame.
#define FrameDivsor 4
ISP_UINT32 frame_div = 4;//9710  S&G delay 3 frame by grouphold 
static ISP_UINT32 gISPFrameCnt = 0;
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Do3AOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAWBOperation_CG2355(void)
{
    switch (gISPFrameCnt % FrameDivsor){
    case 1:
        ISP_IF_AWB_Execute();
        ISP_IF_IQ_SetAWBGains(ISP_IF_AWB_GetGainR(), ISP_IF_AWB_GetGainG(), ISP_IF_AWB_GetGainB(), ISP_IF_AWB_GetGainBase());		
        break;
    case 2:		
        ISP_IF_CALI_Execute();
        break;
    }
    return  MMP_ERR_NONE;
}

//static ISP_UINT32  dgain,s_gain;
#define MAX_SENSOR_GAIN 16
#define ISP_DGAIN_BASE 0x200
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAEOperation_ST
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAEOperation_ST_CG2355(void)
{
#if 0
    ISP_UINT32 s_gain, dgainbase=0x200;
    static ISP_UINT32 dgain;
    ISP_UINT32 H, L;
    ISP_UINT32 new_vsync, new_shutter;
    ISP_UINT32 ExtraVsyncWidth;

    if(ISP_IF_3A_GetSwitch(ISP_3A_ALGO_AE) != 1) return MMP_ERR_NONE;

    switch (gISPFrameCnt % frame_div){
    case 0:
        ISP_IF_AE_Execute();

        //set sensor shutter
        new_vsync = g_SNR_LineCntPerSec * ISP_IF_AE_GetVsync() / ISP_IF_AE_GetVsyncBase();
        new_shutter = g_SNR_LineCntPerSec * ISP_IF_AE_GetShutter() / ISP_IF_AE_GetShutterBase();

        if (new_shutter <= ((g_SNR_LineCntPerSec / 15) - 5)) {
            ExtraVsyncWidth = 0;
        }
        else{
            ExtraVsyncWidth = (new_shutter + 5) - (g_SNR_LineCntPerSec / 15);	
            new_shutter     = ((g_SNR_LineCntPerSec / 15) - 5);
        }
        new_vsync       = (g_SNR_LineCntPerSec / 15);    //fix vsync

        //set sensor gain
        s_gain = ISP_MAX(ISP_IF_AE_GetGain(), ISP_IF_AE_GetGainBase());

        if (s_gain >= ISP_IF_AE_GetGainBase() * 16) {
            dgain = s_gain * dgainbase / (ISP_IF_AE_GetGainBase() * 16);
            s_gain = ISP_IF_AE_GetGainBase() * 16;
        } else {
            dgain  = dgainbase;
        }

        if (s_gain >= ISP_IF_AE_GetGainBase() * 16) {
            H = 15;
            s_gain /= 16;
        }else if (s_gain >= ISP_IF_AE_GetGainBase() * 8) {
            H = 7;
            s_gain /= 8;
        } else if (s_gain >= ISP_IF_AE_GetGainBase() * 4) {
            H = 3;
            s_gain /= 4;
        } else if (s_gain >= ISP_IF_AE_GetGainBase() * 2) {
            H = 1;
            s_gain /= 2;
        } else {
            H = 0;
        }

        L = s_gain * 16 / ISP_IF_AE_GetGainBase() - 16;
			
        if (L > 15) L = 15;
			
        //CG2355 gain nonlinear at following point / 20140225 JS
        if ( (H<<4)+L == 0x10 || (H<<4)+L == 0x30 || (H<<4)+L == 0x70 )
            L = 1;

        //new_s_gain = (L + 16) * ISP_IF_AE_GetGainBase() / 16;
        dgain = s_gain * dgainbase / ((L + 16) * ISP_IF_AE_GetGainBase() / 16);

        // set sensor gain
        gsSensorFunction->MMPF_Sensor_SetReg(0x00, (H<<4)+L);

        // set sensor shutter & vsync		
        gsSensorFunction->MMPF_Sensor_SetReg(0x3E, new_vsync >> 8);
        gsSensorFunction->MMPF_Sensor_SetReg(0x3D, new_vsync);

        gsSensorFunction->MMPF_Sensor_SetReg(0x16, new_shutter >> 8);
        gsSensorFunction->MMPF_Sensor_SetReg(0x10, new_shutter);

        gsSensorFunction->MMPF_Sensor_SetReg(0x2E, ExtraVsyncWidth >> 8);
        gsSensorFunction->MMPF_Sensor_SetReg(0x2D, ExtraVsyncWidth);
        break;
    case 1:
        //ISP_IF_IQ_SetAEGain(dgain, dgainbase);
        break;
    }
#endif
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAEOperation_END
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAEOperation_END_CG2355(void)
{
#if 0
    gISPFrameCnt++;
    if(ISP_IF_3A_GetSwitch(ISP_3A_ALGO_AE) != 1) return MMP_ERR_NONE;

    switch (gISPFrameCnt % frame_div){
    case 1:
        ISP_IF_AWB_GetHWAcc(1);
        break;
    case 3:
        ISP_IF_AE_GetHWAcc(1);
#if (MotionDetection == 1)
        COLOR_StoreAEAcc();
#endif
        break;
    }
#endif
    return  MMP_ERR_NONE;
}


//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAFOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAFOperation_CG2355(void)
{
#if SUPPORT_AUTO_FOCUS
    if(gbAutoFocus == MMP_TRUE){
        //VR_AF_GetAcc();
        ISP_IF_AF_GetHWAcc(1);
        //ISP_IF_R_DoAF();
    }
#endif

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAFOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAFOperation_FrameStart_CG2355(void)
{
#if SUPPORT_AUTO_FOCUS
    if(gbAutoFocus == MMP_TRUE){
        //VR_AF_GetAcc();
        //ISP_IF_R_GetAFAcc();
        //ISP_IF_R_DoAF();
        {
            static ISP_UINT32 frame_cnt = 0;

            switch (frame_cnt++ % 1) {
            case 0:
                ISP_IF_AF_Execute();
                MOTOR_CG2355_VCM_SetAFPosition_Customer(ISP_IF_AF_GetPos(0));
                break;
            }
        }
    }
#endif

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoIQOperation_CG2355
//  Description :
//------------------------------------------------------------------------------
//extern MMP_UBYTE gbSkypeMode;
extern H264_FORMAT_TYPE gbCurH264Type;
static MMP_ERR  MMPF_Sensor_DoIQOperation_CG2355(void)
{
    // switch IQ table if video format is H264
    //PCAM_USB_VIDEO_FORMAT vidFmt = pcam_get_info()->pCamVideoFormat;

    switch (gISPFrameCnt % 2) {
    case 0:
        break;

    case 1:
        // set iq (NR, Edge, CCM, Gamma, etc.) and functions (saturation, contrast, sharpness, hue, etc.)
        ISP_IF_IQ_SetAll();
        ISP_IF_IQ_CheckBypass();

        MMPF_SetPrivacyMask(en_flag, privacymask);
        break;
    }
    return  MMP_ERR_NONE;
}

// Steven ADD
//------------------------------------------------------------------------------
//  Function    : MMPF_SetLightFreq
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_SetLightFreq_CG2355(MMP_USHORT usMode)
{
#if 0

    RTNA_DBG_Str(0, "MMPF_SetLightFreq_CG2355 : ");
    RTNA_DBG_Short(0, usMode);
    RTNA_DBG_Str(0, "\r\n");

    /*
    typedef enum {
        VENUS_FLICKER_MODE_OFF              = 0,
        VENUS_FLICKER_MODE_AUTO             = 1,
        VENUS_FLICKER_MODE_60HZ             = 2,
        VENUS_FLICKER_MODE_50HZ             = 3,
        VENUS_FLICKER_MODE_CAL_BASESHUTTER  = 4
    } VENUS_FLICKER_MODE;
    */

    switch(usMode) {
    case 0:
    case 2:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_60HZ;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_60HZ);
        break;
    case 1:
    case 3:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_50HZ;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_50HZ);
        break;
    default:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_AUTO;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_AUTO);
    }
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_SetStableState_CG2355(MMP_BOOL bStableState)
{
    return  MMP_ERR_NONE;
}

MMP_ERR MMPF_SetHistEQState_CG2355(MMP_BOOL bHistEQState)
{

    return  MMP_ERR_NONE;
}

extern volatile MMP_UBYTE gbDramID;
static MMP_ERR  MMPF_Sensor_SetReg_CG2355(MMP_USHORT usAddr, MMP_USHORT usData)
{
    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_0;  // for AIT8433 70 pin package
    }
    else{
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_1;  // for AIT8451 105 pin package
    }
    MMPF_I2cm_WriteReg(&m_snr_I2cmAttribute, usAddr, usData);
    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_GetReg_CG2355(MMP_USHORT usAddr, MMP_USHORT *usData)
{
    *usData = 0xFF;
    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_0;  // for AIT8433 70 pin package
    }
    else{
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_1;  // for AIT8451 105 pin package
    }
    MMPF_I2cm_ReadReg(&m_snr_I2cmAttribute, usAddr, usData);

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetImageScene_CG2355(MMP_USHORT imgSceneID)
{
#if 0
/*
typedef enum {
    ISP_SCENE_AUTO					= 0,	// scene mode
    ISP_SCENE_PORTRAIT				= 1,	// scene mode
    ISP_SCENE_LANDSCAPE				= 2,	// scene mode
    ISP_SCENE_SPORTS				= 3,	// scene mode
    ISP_SCENE_SUNSET				= 4,	// scene mode
    ISP_SCENE_DUSK					= 5,	// scene mode
    ISP_SCENE_DAWN					= 6,	// scene mode
    ISP_SCENE_NIGHT_SHOT			= 7,	// scene mode
    ISP_SCENE_AGAINST_LIGHT			= 8,	// scene mode
    ISP_SCENE_TEXT					= 9,	// scene mode
    ISP_SCENE_MANUAL				= 10,	// scene mode
    ISP_SCENE_INDOOR				= 11,	// scene mode
    ISP_SCENE_SNOW					= 12,	// scene mode
    ISP_SCENE_FALL					= 13,	// scene mode
    ISP_SCENE_WAVE					= 14,	// scene mode
    ISP_SCENE_FIREWORKS				= 15,	// scene mode
    ISP_SCENE_SHOW_WIN				= 16,	// scene mode
    ISP_SCENE_CANDLE				= 17,	// scene mode
    ISP_SCENE_NONE					= 18	// camera mode
} ISP_SCENE;
*/
    imgSceneID = (imgSceneID % (ISP_SCENE_NONE+1));

    RTNA_DBG_Str(3, "MMPF_SetImageScene_CG2355 : ");
    RTNA_DBG_Byte(3, imgSceneID);
    RTNA_DBG_Str(3, "\r\n");

    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
        MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_F_SetScene(imgSceneID);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAEMode_CG2355(MMP_UBYTE ubAEMode, MMP_UBYTE ubISOMode)
{
#if 0
/*
typedef enum {
    AE_MODE_AUTO						= 0,
    AE_MODE_MANUAL						= 1,
    AE_MODE_ISO							= 2,
    AE_MODE_SHUTTER						= 3
} VENUS_AE_MODE;
*/
/*
typedef enum {
    AE_ISO_AUTO					= 0,
    AE_ISO_50					= 1,
    AE_ISO_100					= 2,
    AE_ISO_200					= 3,
    AE_ISO_400					= 4,
    AE_ISO_800					= 5,
    AE_ISO_1600					= 6,
    AE_ISO_3200					= 7,
    ISP_AE_ISO_NUM
} VENUS_AE_ISO;
*/
    #if 0 //FIXME, VSN_V2
    if (ubAEMode == 3)
        ISP_IF_AE_SetISO(ISP_AE_MODE_AUTO);
    else
    #endif
        ISP_IF_AE_SetISO(ubISOMode);
#endif
	
    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAFMode_CG2355(MMP_UBYTE ubAFMode)
{
#if 0
/*
typedef enum {
    VENUS_AF_MODE_AUTO					= 0,
    VENUS_AF_MODE_MANUAL				= 1,
    VENUS_AF_MODE_MACRO					= 2,
    VENUS_AF_MODE_FULL					= 3,
    VENUS_AF_MODE_MOTOR_TEST            = 4,
    VENUS_AF_MODE_NULL					= 5
} VENUS_AF_MODE;
*/
    RTNA_DBG_Str(3, "MMPF_Sensor_SetAFMode_CG2355 : ");
    RTNA_DBG_Byte(3, ubAFMode);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.AutoFocusMode = (MMP_UBYTE)ubAFMode;
    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
    //	MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_AF_SetMode(ubAFMode);
#endif

    return  MMP_ERR_NONE;
}

//======================================================================//
// function : MMPF_Sensor_SetImageEffect                                //
// parameters:                                                          //
// return :                                                             //
// description :                                                        //
//======================================================================//
static MMP_ERR  MMPF_Sensor_SetImageEffect_CG2355(MMP_USHORT imgEffectID)
{
#if 0
/*
typedef enum {
    ISP_IMAGE_EFFECT_NORMAL			= 0,
    ISP_IMAGE_EFFECT_GREY			= 1,
    ISP_IMAGE_EFFECT_SEPIA			= 2,
    ISP_IMAGE_EFFECT_NEGATIVE		= 3,
    ISP_IMAGE_EFFECT_ANTIQUE		= 4,
    ISP_IMAGE_EFFECT_WATERCOLOR		= 5,
    ISP_IMAGE_EFFECT_PORTRAIT		= 6,
    ISP_IMAGE_EFFECT_LANDSCAPE		= 7,
    ISP_IMAGE_EFFECT_SUNSET			= 8,
    ISP_IMAGE_EFFECT_DUSK			= 9,
    ISP_IMAGE_EFFECT_DAWN			= 10,
    ISP_IMAGE_EFFECT_RED			= 11,
    ISP_IMAGE_EFFECT_GREEN			= 12,
    ISP_IMAGE_EFFECT_BLUE			= 13,
    ISP_IMAGE_EFFECT_YELLOW			= 15,
    ISP_IMAGE_EFFECT_EMBOSS			= 17,
    ISP_IMAGE_EFFECT_OIL			= 18,
    ISP_IMAGE_EFFECT_BW				= 19,
    ISP_IMAGE_EFFECT_SKETCH			= 20,
    ISP_IMAGE_EFFECT_CRAYONE		= 21,
    ISP_IMAGE_EFFECT_WHITEBOARD		= 22,
    ISP_IMAGE_EFFECT_BLACKBOARD		= 23,
    ISP_IMAGE_EFFECT_VIVID			= 24,
    ISP_IMAGE_EFFECT_NUM
} ISP_IMAGE_EFFECT;
*/

    imgEffectID = (imgEffectID % (ISP_IMAGE_EFFECT_NUM+1));

    RTNA_DBG_Str(3, "MMPF_Sensor_SetImageEffect_CG2355 : ");
    RTNA_DBG_Byte(3, imgEffectID);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.ImageEffect = (MMP_USHORT)imgEffectID;
    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
    //	MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_F_SetImageEffect(imgEffectID);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAWBType_CG2355(MMP_UBYTE ubType)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetAWBType_CG2355 : ");
    RTNA_DBG_Byte(3, ubType);
    RTNA_DBG_Str(3, "\r\n");

    cAWBtype = ubType;
#endif
    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetContrast_CG2355(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_Contrast_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.ContrastLevel = ubLevel;
    ISP_IF_F_SetContrast(ubLevel);
#endif

    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetSaturation_CG2355(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetSaturation_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.SaturationLevel = ubLevel;
    ISP_IF_F_SetSaturation(ubLevel);
#endif

    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetSharpness_CG2355(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetSharpness_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.SharpnessLevel = ubLevel;
    ISP_IF_F_SetSharpness(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetHue_CG2355(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetHue_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_F_SetHue(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetGamma_CG2355(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetGamma_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_F_SetGamma(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

/*static MMP_ERR  MMPF_Sensor_SetBacklight_CG2355(MMP_UBYTE ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetBacklight_CG2355 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //ISP_IF_F_SetBacklight(ubLevel);
    if(ubLevel > 1){
        ISP_IF_F_SetWDR(ISP_WDR_DISABLE);
    }
    else{
        ISP_IF_F_SetWDR(ISP_WDR_ENABLE);
    }
#endif

    return  MMP_ERR_NONE;
}

*/

static void  MMPF_Sensor_SetCaptureISPSetting_CG2355(MMP_UBYTE usCaptureBegin)
{
#if 0
    AITPS_VIF   pVIF = AITC_BASE_VIF;
    MMP_ULONG   ulVIF_Width, ulVIF_Height;
    //MMP_UBYTE   vif_id = 0;

    /* ToDo: setting IOS and ISP windows*/
    if(MMP_TRUE == usCaptureBegin){
        ulVIF_Width = (pVIF->VIF_GRAB[vif_id].PIXL_ED - pVIF->VIF_GRAB[vif_id].PIXL_ST + 1);
        ulVIF_Height = (pVIF->VIF_GRAB[vif_id].LINE_ED - pVIF->VIF_GRAB[vif_id].LINE_ST + 1);
        //gSensorFunc.SetISPWindows(ulVIF_Width,ulVIF_Height);
    }
#endif
	
    return;
}

/*Prepare for raw preview zoom*/
static void  MMPF_Sensor_SetISPWindow_CG2355(MMP_ULONG ulWidth, MMP_ULONG ulHeight)
{
    //gSensorFunc.SetISPWindows(ulWidth,ulHeight);
    return;
}

/*Check AIT can preview in this resolution*/
static MMP_UBYTE  MMPF_Sensor_CheckPreviewAbility_CG2355(MMP_USHORT usPreviewmode)
{
#if 0
    switch(usPreviewmode){
        case 0:                 //5M can't preview
            return MMP_TRUE;
            break;
        case 1:                 //1.3M can preview
            return MMP_TRUE;
            break;
    }
#endif
	
    return MMP_TRUE;            //5M and 1.3M always can preview
}

static void SetAFWin_CG2355(MMP_USHORT usIndex, MMP_USHORT usPreviewWidth, MMP_USHORT usPreviewHeight,
					MMP_USHORT  usStartX, MMP_USHORT  usStartY, MMP_USHORT  usWidth, MMP_USHORT  usHeight)
{

    //DBG_S3("SetAFWin (TODO)\r\n");
}

//static MMP_BOOL VGAPreview = MMP_TRUE;
static void SetAFEnable_CG2355(MMP_UBYTE enable)
{
    //if (enable) {
        //RTNA_DBG_Str(3, "VENUS_ISP_AUTO_FOCUS_STATUS_START\r\n");
        ISP_IF_AF_Control(ISP_AF_START);
        //actually calling SNR_CG2355_SetAutoFocusControl()
    //} else {
        //RTNA_DBG_Str(3, "VENUS_ISP_AUTO_FOCUS_STATUS_STOP\r\n");
        //ISP_IF_AF_Control(ISP_AF_STOP);
    //}
}

static MMP_ERR MMPF_Sensor_SetAEEnable_CG2355(MMP_UBYTE bEnable)
{
    /*
    if (bEnable) {
        g3AConfig.AE_Enabled     = ON;
    }
    else {
        g3AConfig.AE_Enabled     = OFF;
    }*/

    ISP_IF_3A_SetSwitch(ISP_3A_ALGO_AE, bEnable);

    return MMP_ERR_NONE;
}

static MMP_ERR MMPF_Sensor_SetAWBEnable_CG2355(MMP_UBYTE bEnable)
{
#if 0
    if (bEnable) {
        g3AConfig.AWB_Enabled    = ON;
        g3AConfig.AWBRGB_Enabled = ON;
    }
    else {
        g3AConfig.AWB_Enabled    = OFF;
        g3AConfig.AWBRGB_Enabled = OFF;
    }
#endif

    if(bEnable)
        ISP_IF_AWB_SetMode(ISP_AWB_MODE_AUTO);
    else
        ISP_IF_AWB_SetMode(ISP_AWB_MODE_MANUAL);

    return MMP_ERR_NONE;
}
static void SetAEsmooth_CG2355(MMP_UBYTE smooth)  {return;}
static MMP_ERR MMPF_Sensor_SetExposureValue_CG2355(MMP_UBYTE ev)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetExposureValue_CG2355 : ");
    //RTNA_DBG_Byte(3, gISPConfig.EV);
    //RTNA_DBG_Str(3, " : ");
    RTNA_DBG_Byte(3, ev);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_AE_SetEV(ev);
#endif

    return MMP_ERR_NONE;
}

static MMP_UBYTE GetAFPosition_CG2355     (void){ return 0;/*volatile MMP_UBYTE *GPIO_BASE_B = (volatile MMP_UBYTE *)0x80009400;  return GPIO_BASE_B[0xB0];*/}


static MMP_UBYTE GetAFEnable_CG2355(void)
{
    //return 0;
    return ISP_IF_AF_GetResult();
}

static MMP_UBYTE GetExposureValue_CG2355  (void){return ISP_IF_AE_GetEV();}
static MMP_UBYTE GetAEsmooth_CG2355       (void){return 0;}
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Set3AStatus
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR	MMPF_Sensor_Set3AStatus_CG2355(MMP_BOOL bEnable)
{
    #if 1
    MMPF_Sensor_3A_SetInterrupt(bEnable);
    //#else
    if (bEnable)
        ISP_IF_3A_Control(ISP_3A_ENABLE);
    else
        ISP_IF_3A_Control(ISP_3A_DISABLE);
    #endif
    return	MMP_ERR_NONE;
}

static void  MMPF_Sensor_SetColorID_CG2355(MMPF_SENSOR_ROTATE_TYPE RotateType)
{	    
    //ISP_IF_3A_Control(ISP_3A_PAUSE);
    switch(RotateType) {
    case MMPF_SENSOR_ROTATE_NO_ROTATE:
        gsSensorFunction->MMPF_Sensor_SetReg(0x17,0x00);
        ISP_IF_IQ_SetColorID(0);
        break;
    case MMPF_SENSOR_ROTATE_RIGHT_90:
        break;
    case MMPF_SENSOR_ROTATE_RIGHT_180:
        gsSensorFunction->MMPF_Sensor_SetReg(0x17,0x03);
        ISP_IF_IQ_SetColorID(3);
        break;
    case MMPF_SENSOR_ROTATE_RIGHT_270:
        break;
    case MMPF_SENSOR_ROTATE_H_MIRROR:
        gsSensorFunction->MMPF_Sensor_SetReg(0x17,0x01);
        ISP_IF_IQ_SetColorID(1);
        break;
    case MMPF_SENSOR_ROTATE_V_FLIP:
        gsSensorFunction->MMPF_Sensor_SetReg(0x17,0x02);
        ISP_IF_IQ_SetColorID(2);
        break;
    }
    ISP_IF_IQ_SetDirection((ISP_UINT8)RotateType);
    //ISP_IF_3A_Control(ISP_3A_RECOVER);
    return;
}

static void MMPF_Sensor_SetSensorGain_CG2355(MMP_ULONG gain)
{
    MMP_USHORT s_gain;
    MMP_ULONG  H, L;

    s_gain = VR_MIN(VR_MAX(gain, 64), 511); // API input gain range : 64~511, 64=1X

    if (s_gain >= 64 * 8) {
        H = 7;
    } else if (s_gain >= 64 * 4) {
        H = 3;
    } else if (s_gain >= 64 * 2) {
        H = 1;
    } else {
        H = 0;
    }

    L = s_gain * 16 / 64 / (H+1) - 16;

    if (L > 15) L = 15;	

    // set sensor gain
    MMPF_Sensor_SetReg_CG2355(0xB0, (H<<4)+L);

}

static void MMPF_Sensor_SetSensorShutter_CG2355(MMP_ULONG shutter, MMP_ULONG vsync)
{
    //ISP_UINT32 new_vsync = g_SNR_LineCntPerSec * ISP_IF_AE_GetVsync() / ISP_IF_AE_GetVsyncBase();
    ISP_UINT32 new_shutter = g_SNR_LineCntPerSec * ISP_IF_AE_GetShutter() / ISP_IF_AE_GetShutterBase();

    //new_vsync   = VR_MAX(vsync, shutter + 5);  //+8
    new_shutter = 1;//VR_MIN(VR_MAX(shutter, 1), new_vsync - 2);//2);

    //MMPF_Sensor_SetReg_CG2355(0x43, new_vsync >> 8);
    //MMPF_Sensor_SetReg_CG2355(0x42, new_vsync);

    MMPF_Sensor_SetReg_CG2355(0x03, new_shutter >> 8);
    MMPF_Sensor_SetReg_CG2355(0x04, new_shutter);
    //Exposure[13:8]:0x03[5:0]  Exposure[7:0]:0x04
}

MMPF_SENSOR_FUNCTION  SensorFunction_CG2355 =
{
    MMPF_Sensor_Initialize_CG2355,
    MMPF_Sensor_InitializeVIF_CG2355,
    MMPF_Sensor_InitializeISP_CG2355,
    MMPF_Sensor_PowerDown_CG2355,
    MMPF_Sensor_SetFrameRate_CG2355,
    MMPF_Sensor_ChangeMode_CG2355,
    MMPF_Sensor_ChangePreviewMode_CG2355,
    MMPF_Sensor_SetPreviewMode_CG2355,
    MMPF_Sensor_SetReg_CG2355,
    MMPF_Sensor_GetReg_CG2355,
    MMPF_Sensor_DoAWBOperation_CG2355,
    MMPF_Sensor_DoAEOperation_ST_CG2355,
    MMPF_Sensor_DoAEOperation_END_CG2355,
    MMPF_Sensor_DoAFOperation_CG2355,
    MMPF_Sensor_DoAFOperation_FrameStart_CG2355,
    MMPF_Sensor_DoIQOperation_CG2355,
    MMPF_Sensor_SetImageEffect_CG2355,
    MMPF_SetLightFreq_CG2355,
    MMPF_SetStableState_CG2355,
    MMPF_SetHistEQState_CG2355,
    MMPF_Sensor_SetAFPosition_CG2355,
    SetAFWin_CG2355,
    SetAFEnable_CG2355,
    MMPF_Sensor_SetAEEnable_CG2355,
    MMPF_Sensor_SetAWBEnable_CG2355,
    MMPF_Sensor_SetExposureValue_CG2355,
    SetAEsmooth_CG2355,
    MMPF_Sensor_SetImageScene_CG2355,
    GetAFPosition_CG2355,
    GetAFEnable_CG2355,
    GetExposureValue_CG2355,
    GetAEsmooth_CG2355,
    MMPF_Sensor_SetAWBType_CG2355,
    MMPF_Sensor_SetContrast_CG2355,
    MMPF_Sensor_SetSaturation_CG2355,
    MMPF_Sensor_SetSharpness_CG2355,
    MMPF_Sensor_SetHue_CG2355,
    MMPF_Sensor_SetGamma_CG2355,
    //MMPF_Sensor_SetBacklight_CG2355,
    MMPF_Sensor_SetAEMode_CG2355,
    MMPF_Sensor_SetAFMode_CG2355,
    MMPF_Sensor_SetCaptureISPSetting_CG2355,
    MMPF_Sensor_SetISPWindow_CG2355,
    MMPF_Sensor_CheckPreviewAbility_CG2355,
    MMPF_Sensor_Set3AStatus_CG2355,
    MMPF_Sensor_SetColorID_CG2355,
    MMPF_Sensor_SetSensorGain_CG2355,
    MMPF_Sensor_SetSensorShutter_CG2355
};


MMP_USHORT SNR_CG2355_Reg_Init_Customer[] =
{
    ////20140926
    /////////////////////////////////////////////////////
    //////////////////////	 SYS   //////////////////////
    /////////////////////////////////////////////////////
    0xfe,0x80,
    0xfe,0x80,
    0xfe,0x80,
    0xf2,0x00,
    0xf6,0x00,

    0xf7,0x31,

    0xf8,0x06,
    0xf9,0x0e,
    0xfa,0x00,
    0xfc,0x06,
    0xfe,0x00,

    /////////////////////////////////////////////////////
    ///////////////    ANALOG & CISCTL    ///////////////
    /////////////////////////////////////////////////////
    0x03,0x01,//0x07 //ZDW_BARCODE
    0x04,0xb0,
    0x05,0x01,//0x03,
    0x06,0x1c,//0x4c,
    0x07,0x00,
    0x08,0x12,
    0x0a,0x00,
    0x0c,0x04,
    0x0d,0x04,
    0x0e,0xc0,
    0x0f,0x06,
    0x10,0x50,
    0x17,0x17,
    0x19,0x0b,
    0x1b,0x48,
    0x1c,0x12,
    0x1d,0x10,
    0x1e,0xbc,
    0x1f,0xc9,
    0x20,0x71,
    0x21,0x20,
    0x22,0xa0,
    0x23,0x51,
    0x24,0x19,
    0x27,0x20,
    0x28,0x00,
    0x2b,0x80,
    0x2c,0x38,
    0x2e,0x16,
    0x2f,0x14,
    0x30,0x00,
    0x31,0x01,
    0x32,0x02,
    0x33,0x03,
    0x34,0x07,
    0x35,0x0b,
    0x36,0x0f,

    /////////////////////////////////////////////////////
    //////////////////////   MIPI   /////////////////////
    /////////////////////////////////////////////////////
    0xfe, 0x03,
    0x01, 0x87,
    0x02, 0x00,
    0x03, 0x90,
    0x04, 0x01,
    0x05, 0x00,
    0x06, 0xa2,
    0x10, 0x81,
    0x11, 0x2b,
    0x12, 0xd0,
    0x13, 0x07,
    0x15, 0x60,

    0x21, 0x10,
    0x22, 0x03,
    0x23, 0x20,
    0x24, 0x02,
    0x25, 0x10,
    0x26, 0x08,
    0x27, 0x06,
    0x29, 0x02,
    0x2a, 0x0a,
    0x2b, 0x08,

    0x40, 0x00,
    0x41, 0x00,
    0x42, 0x40,
    0x43, 0x06,
    0xfe, 0x00,

    /////////////////////////////////////////////////////
    //////////////////////	 gain   /////////////////////
    /////////////////////////////////////////////////////
    0xb0,0x50,
    0xb1,0x01,
    0xb2,0x00,
    0xb3,0x40,
    0xb4,0x40,
    0xb5,0x40,
    0xb6,0x00,

    /////////////////////////////////////////////////////
    //////////////////////   crop   /////////////////////
    /////////////////////////////////////////////////////
    0x92,0x02,
    0x94,0x00,
    0x95,0x04,
    0x96,0xb0,
    0x97,0x06,
    0x98,0x40,

    /////////////////////////////////////////////////////
    //////////////////////    BLK   /////////////////////
    /////////////////////////////////////////////////////
    0x18,0x02,
    0x1a,0x01,
    0x40,0x42,
    0x41,0x00,
    0x44,0x00,
    0x45,0x00,
    0x46,0x00,
    0x47,0x00,
    0x48,0x00,
    0x49,0x00,
    0x4a,0x00,
    0x4b,0x00,
    0x4e,0x3c,
    0x4f,0x00,
    0x5e,0x00,
    0x66,0x20,
    0x6a,0x02,
    0x6b,0x02,
    0x6c,0x02,
    0x6d,0x02,
    0x6e,0x02,
    0x6f,0x02,
    0x70,0x02,
    0x71,0x02,

    /////////////////////////////////////////////////////
    ////////////////////  dark sun  /////////////////////
    /////////////////////////////////////////////////////
    0x87,0x03,
    0xe0,0xe7,
    0xe3,0xc0,

    /////////////////////////////////////////////////////
    //////////////////////   MIPI   /////////////////////
    /////////////////////////////////////////////////////
    0xfe, 0x03,

    0x10, 0x91,

    0xfe, 0x00
};
void SNR_CG2355_InitSensor_Customer(void) {
    // implement your initialize sensor routine here ==============================================
    ISP_UINT32 i;

    MMPF_OS_Sleep_MS(50); 

    // set register...........
    RTNA_DBG_Str(0, "set sensor reg\r\n");
    for (i = 0; i < sizeof(SNR_CG2355_Reg_Init_Customer)/4; i++) {
        gsSensorFunction->MMPF_Sensor_SetReg(SNR_CG2355_Reg_Init_Customer[i*2], SNR_CG2355_Reg_Init_Customer[i*2+1]);
        MMPF_OS_Sleep_MS(1);
    }
}


void SNR_CG2355_SetSensorAddMode_Customer(ISP_UINT8 enable)
{

}
MMP_USHORT SNR_CG2355_Reg_1280x720_Customer[] = 
{
0,0
};

void SNR_CG2355_SetSensorResolution_Customer(ISP_SENSOR_RESOL res)
{
    // implement your change sensor resolution routine here =======================================

    MMP_ULONG VIFGrab_H_Start, VIFGrab_H_Length, VIFGrab_V_Start, VIFGrab_V_Length;

    switch (res) {
    case ISP_SENSOR_RESOL_1600x1200:
    case ISP_SENSOR_RESOL_1280x960:
    case ISP_SENSOR_RESOL_1280x720:

        // set register...........
        //for (i = 0; i < VR_ARRSIZE(SNR_CG2355_Reg_1280x720_Customer)/2; i++) {
        //    gsSensorFunction->MMPF_Sensor_SetReg(SNR_CG2355_Reg_1280x720_Customer[i*2], SNR_CG2355_Reg_1280x720_Customer[i*2+1]);
        //}

        // set target fps and corresponding sensor vsync
        //ISP_IF_AE_SetSensorVsync(828, 30*10);//bossino fix m2ts AV sync @0928 ISP_IF_SNR_SetFPS(0x0348, 30);

        // set vif grab range which is fetched to ISP (for lens shading and 3A window calculation)
        VIFGrab_H_Start     = 1;
        VIFGrab_H_Length    = gsSensorPreviewWidth[1]+8;
        VIFGrab_V_Start     = 1;
        VIFGrab_V_Length    = gsSensorPreviewHeight[1]+8;

        g_SNR_LineCntPerSec = 24840; //207 * 120;    //???? 1sec H_s

        ISP_IF_AE_SetMaxSensorFPSx10(300);
        ISP_IF_IQ_SetISPInputLength(VIFGrab_H_Length, VIFGrab_V_Length);

        // set scaler downsample rate (for scaler calculation)
        //ISP_IF_SNR_SetDownSample(1, 1);

        // set color id
        #if SENSOR_ROTATE_180
        ISP_IF_IQ_SetColorID(3);
        #else
        ISP_IF_IQ_SetColorID(0);// mirror is 3
        #endif

        // calcuate shading ratio
        {
            ISP_UINT32 base_x, base_y;
            ISP_UINT32 x_scale_n = 0x200;
            ISP_UINT32 y_scale_n = 0x200;
            ISP_IF_IQ_GetCaliBase(&base_x, &base_y);

            x_scale_n = VIFGrab_H_Length * 0x200 / base_x;
            y_scale_n = VIFGrab_V_Length * 0x200 / base_y;

            ISP_IF_IQ_SetCaliRatio(	x_scale_n,  //x_scale_n, 
                                    0x200,      //x_scale_m, 
                                    0,          //x_offset, 
                                    y_scale_n,  //y_scale_n, 
                                    0x200,      //y_scale_m, 
                                    0);         //y_offset
        }
        break;
    }
	
    // end of your implementation =================================================================

    // set VIF OPR (Actual sensor output size)
    {
        AITPS_VIF  pVIF = AITC_BASE_VIF;
        volatile MMP_BYTE* pISP = (volatile MMP_BYTE*)AITC_BASE_ISP;
        //MMP_UBYTE   vif_id = 0;

        pVIF->VIF_GRAB[vif_id].PIXL_ST = VIFGrab_H_Start;
        pVIF->VIF_GRAB[vif_id].PIXL_ED = VIFGrab_H_Start + VIFGrab_H_Length -1;
        pVIF->VIF_GRAB[vif_id].LINE_ST = VIFGrab_V_Start ;
        pVIF->VIF_GRAB[vif_id].LINE_ED = VIFGrab_V_Start + VIFGrab_V_Length -1;
        pVIF->VIF_INT_LINE_NUM_0[vif_id] = VIFGrab_V_Length - 31;//61;//121;

#if 1	
        dbg_printf(3,"ViF(xs,xe,ys,ye)=(%d,%d,%d,%d)\r\n",pVIF->VIF_GRAB[vif_id].PIXL_ST,pVIF->VIF_GRAB[vif_id].PIXL_ED,pVIF->VIF_GRAB[vif_id].LINE_ST,pVIF->VIF_GRAB[vif_id].LINE_ED);
        dbg_printf(3, "[0] VIFGrab_H_Length = %d, VIFGrab_V_Length = %d\r\n", VIFGrab_H_Length, VIFGrab_V_Length); 
#endif

        if (m_gsISPCoreID == 868) {
            // ISP pipeline selection
            //APICAL_XBYTE[APICAL_CTL_REG1] &= ~(0x80); // clear auto size
            pISP[0x0B] = 0x30;
            pISP[0x09] = 0x0C;

            // IP ColorID setting
            //pISP[0x09] &= ~(0xF0);
            //pISP[0x09] |= (ISP_IF_SNR_GetColorID() == 0 ? 3 : (ISP_IF_SNR_GetColorID() == 1 ? 2 : (ISP_IF_SNR_GetColorID() == 2 ? 1: 0))) * (0x50);
        }
    }

    //================
    // set iq (NR, Edge, CCM, Gamma, etc.) and functions (saturation, contrast, sharpness, hue, etc.)
    ISP_IF_IQ_SetAll();

    // SetBeforePreview and SetBeforeCapture should be called after getting new configurations of the current resolution
    if (gSystemMode  == VENUS_SYSTEM_MODE_SNAPSHOT) {
        // update AE before capture
        ISP_IF_AE_UpdateBeforeCapture();

        // update AWB before capture
        ISP_IF_AWB_UpdateBeforeCapture();

        // special settings for capture could be set here =============================================
        {
        }
        // end of special settings ====================================================================
    } else if (gSystemMode == VENUS_SYSTEM_MODE_PREVIEW) {
        // update AE before preview
        ISP_IF_AE_UpdateBeforePreview();

        // update AWB before preview
        ISP_IF_AWB_UpdateBeforePreview();
    }

    // set direction (color ID, orientation, etc.)
    ISP_IF_IQ_SetDirection(ISP_IQ_DIRECTION_ORIGINAL);

    // set exposure parameters
    ISP_IF_IQ_SetAWBGains(ISP_IF_AWB_GetGainR(), ISP_IF_AWB_GetGainG(), ISP_IF_AWB_GetGainB(), ISP_IF_AWB_GetGainBase());

    // update ISP window
    ISP_IF_IQ_UpdateInputSize();

    // check if modules are bypassed
    ISP_IF_IQ_CheckBypass();

    // force update iq to hardware
    ISP_IF_IQ_UpdateOprtoHW(ISP_IQ_SWITCH_ALL, 1);

}
#if (MotionDetection == 1)
static MMP_ULONG AEAcc_temp[9]={0xFFFF};
extern MMP_ULONG AEAcc_Buffer[9];
void COLOR_StoreAEAcc(void)
{
    MMP_SHORT i;
    for(i=0;i<9;i++)
    {
        AEAcc_temp[i] = AEAcc_Buffer[i];
    }
}

void COLOR_CopyAEAcc(MMP_ULONG* addr)
{
    struct _FRAME_H264_PH *header = (struct _FRAME_H264_PH *)addr;
    MMP_SHORT i;
    for(i=0;i<9;i++)
    {
    	header->dwAEAcc[i] = AEAcc_temp[i];
    }
    header->dwAELightCond = ISP_IF_AE_GetLightCond();
    header->dwAWB_GainRValue = ISP_IF_AWB_GetGainR();
    header->dwAWB_GainBValue  = ISP_IF_AWB_GetGainB();
}
#endif

#if SUPPORT_AUTO_FOCUS
void MOTOR_CG2355_VCM_InitMotor_Customer(void)
{
    //uint16 ret;
}
void MOTOR_CG2355_VCM_EnterStandby_Customer(MMP_USHORT af_pos)
{
    //VR_Motor_SmoothMove(af_pos);

}

void MOTOR_CG2355_VCM_SetAFPosition_Customer(MMP_USHORT af_pos)
{

    //dbg_printf(3, "af_pos = %d\r\n", af_pos);
}

MMP_UBYTE MOTOR_CG2355_VCM_CheckAFreachPosition_Customer(MMP_USHORT af_pos)
{
    
    return 1;
    
}
#endif

#if (SENSOR_ID_CG2355== 0)
MMPF_SENSOR_FUNCTION  *SensorFunction_Module0 = &SensorFunction_CG2355;
#endif

#if (SENSOR_ID_CG2355== 1)
MMPF_SENSOR_FUNCTION  *SensorFunction_Module1 = &SensorFunction_CG2355;
#endif

void MMPF_SetPrivacyMask(ISP_UINT8 en, ISP_UINT8* privacymask){

    volatile MMP_UBYTE* REG_BASE_B = (volatile MMP_UBYTE*)0x80000000;
    ISP_UINT16 i, j;
    ISP_UINT8 cs_bank[4] = {0, 4, 168, 172};

    ISP_UINT16 cslut_offset[7*5]= {   0,   8,  16,  24,  32,  40,  48,	
                                    336, 344, 352, 360, 368, 376, 384,
                                    672, 680, 688, 696, 704, 712, 720,
                                   1008,1016,1024,1032,1040,1048,1056,
                                   1344,1352,1360,1368,1376,1384,1392 };

    if(en ==0) return;
	
    for(i = 0; i < 5; i++){
        for(j = 0; j < 7; j++){
            if(privacymask[i * 7 + j]==1){//((i ==2 && j == 3) || (i ==0 && j == 0)){//
                *((volatile MMP_U_LONG*)(*((volatile MMP_U_LONG*)0x80000DF8) + cslut_offset[7 * i + j] + cs_bank[0])) = 0;
                *((volatile MMP_U_LONG*)(*((volatile MMP_U_LONG*)0x80000DF8) + cslut_offset[7 * i + j] + cs_bank[1])) = 0;
                *((volatile MMP_U_LONG*)(*((volatile MMP_U_LONG*)0x80000DF8) + cslut_offset[7 * i + j] + cs_bank[2])) = 0;			
                *((volatile MMP_U_LONG*)(*((volatile MMP_U_LONG*)0x80000DF8) + cslut_offset[7 * i + j] + cs_bank[3])) = 0;							
            }
        }
    }	
    REG_BASE_B[0xdf0] |= 0x02;
}


#endif  //BIND_SENSOR_CG2355
#endif
